#!/usr/bin/env python

import roslib; roslib.load_manifest('framer')
import rospy
import actionlib
from pr2_hri.msg import HriAction, HriGoal
import smach
from std_msgs.msg import String

class RequestInteraction(smach.State):
    def __init__(self, cue = None):
        smach.State.__init__(
            self,
            outcomes=['success', 'fail'],
            output_keys=[],
            input_keys=['cue']
        )
        self.cue = cue
        self.client = actionlib.SimpleActionClient('HriActionServer', HriAction)
        rospy.loginfo('Waiting for HRI Server')
        self.client.wait_for_server()
        rospy.loginfo('Connected with HRI Server')

    def execute(self, data):
        cue=self.cue if self.cue else data.cue
        if not cue:
            rospy.logerr(
                'Was supposed to request interaction, but was not given a cue.')
            return 'fail'
        goal = HriGoal
        goal.goalString = String(cue)
        self.client.send_goal(goal)
        # DNC about results because HRI team says it always succeeds
        self.client.wait_for_result()
        return 'success'
